#include "AgvTask.h"
#include <ExtSRAMInterface.h>
// #include <DCMotor.h>
#include <Command.h>
#include "Command.h"
#include "infrare.h"
#include "Ultrasonic.h"
#include "Drive.h"
#include "SYN7318.h"

_TASK::_TASK()
{
}

_TASK::~_TASK()
{
}

void _TASK::AGV_Path_Analysis()
{
    uint8_t dir_bef = 2; // 初始化当前车头方向，0向上，1向右，2向下，3向左//将局部变量改为全局变量
    char path1[] =
        {
            "D7,D6,F6,F4,F2,D2,D1"};
    char *p = path1;
    uint8_t Special_Tpg_Flag; // 特殊地形标准位
    Serial.print("1\n");
    // 路径读取到坐标数组中
    do
    {
        link[linked_list].Cd_X = *p;                 //**x为A-G
        link[linked_list].Cd_Y = *(p + 1);           //**Y为1-7
        link[linked_list].k = linked_list;           //**节点对应任务
        link[linked_list].vernier = linked_list + 1; //**链表下一个结点
        linked_list++;
        p = p + 3;
    } while (*(p - 1) == ',');

    link[linked_list].Cd_X = '\0';
    link[linked_list].Cd_Y = '\0';

    // 路径解析0
    for (linked_list = 0; link[linked_list].Cd_X != '\0'; linked_list = link[linked_list].vernier)
    {
        /****************做任务*******************/
#if 1
        Current_Task(link[linked_list].k); // 执行本点对应任务
#endif
        /******************end********************/
        if (link[link[linked_list].vernier].Cd_X) //**link下一个结点的x轴不为0  //不是最后一个结点就执行相应代码	link[++linked_list].Cd_X
        {
            uint8_t choose = Quick_reading_FS(linked_list); // 判断相对的位置

            char arr[16];
            if (choose == 2 && tracking == 1) // 判断列坐标大小，如果后者大于前者，则后者在前者左边，调整后方向为3
            {
                Change_dir(dir_bef, 3); // 调整车头方向
                dir_bef = 3;
                DCMotor.CarTrack(Go_Speed); // 车辆循迹
                delay(100);
                DCMotor.Go(Go_Speed, Go_Temp); // 车辆前进
                delay(500);
            }
            else if (choose == 1 && tracking == 1) // 判断列坐标大小，如果后者小于前者，则后者在前者右边，调整后方向为1
            {
                Change_dir(dir_bef, 1); // 调整车头方向
                dir_bef = 1;
                DCMotor.CarTrack(Go_Speed); // 车辆循迹
                delay(100);
                DCMotor.Go(Go_Speed, Go_Temp); // 车辆前进
                delay(500);
            }

            else if (choose == 4 && tracking == 1) // 后者行坐标小于前者，则后者在前者下方，调整后方向为2
            {
                Change_dir(dir_bef, 2); // 调整车头方向
                dir_bef = 2;
                DCMotor.CarTrack(Go_Speed); // 车辆循迹
                delay(100);
                DCMotor.Go(Go_Speed, Go_Temp); // 车辆前进
            }
            else if (choose == 3 && tracking == 1) // 后者行坐标大于前者，则后者在前者上方，调整后方向为0
            {
                Change_dir(dir_bef, 0); // 调整车头方向
                dir_bef = 0;
                DCMotor.CarTrack(Go_Speed); // 车辆循迹
                delay(100);
                DCMotor.Go(Go_Speed, Go_Temp); // 车辆前进
            }
        }
        tracking = 1;
    }
    if (link[linked_list].Cd_X == 0)
    {
        Send_Voice_Data(Voice_Finish);
        To_Car(0x1d, 0x00, 0x00, 0x00);
        return;
    }
}
void _TASK::AGV_Path_Analysis_Video()
{
    uint8_t dir_bef = 2; // 初始化当前车头方向，0向上，1向右，2向下，3向左//将局部变量改为全局变量
    char path1[] =
        {
            "D7,D6,F6,F4,F2,D2,D1"};
    char *p = path1;
    uint8_t Special_Tpg_Flag; // 特殊地形标准位
    Serial.print("1\n");
    // 路径读取到坐标数组中
    do
    {
        link[linked_list].Cd_X = *p;                 //**x为A-G
        link[linked_list].Cd_Y = *(p + 1);           //**Y为1-7
        link[linked_list].k = linked_list;           //**节点对应任务
        link[linked_list].vernier = linked_list + 1; //**链表下一个结点
        linked_list++;
        p = p + 3;
    } while (*(p - 1) == ',');

    link[linked_list].Cd_X = '\0';
    link[linked_list].Cd_Y = '\0';

    // 路径解析0
    for (linked_list = 0; link[linked_list].Cd_X != '\0'; linked_list = link[linked_list].vernier)
    {
        /****************做任务*******************/
#if 1
        Current_Task(link[linked_list].k); // 执行本点对应任务
#endif
        /******************end********************/
        if (link[link[linked_list].vernier].Cd_X) //**link下一个结点的x轴不为0  //不是最后一个结点就执行相应代码	link[++linked_list].Cd_X
        {
            uint8_t choose = Quick_reading_FS(linked_list); // 判断相对的位置

            char arr[16];
            if (choose == 2 && tracking == 1) // 判断列坐标大小，如果后者大于前者，则后者在前者左边，调整后方向为3
            {
                Change_dir(dir_bef, 3); // 调整车头方向
                dir_bef = 3;
                OpenMV_Track(50);
                delay(100);
                DCMotor.Go(Go_Speed, Go_Temp); // 车辆前进
                delay(500);
            }
            else if (choose == 1 && tracking == 1) // 判断列坐标大小，如果后者小于前者，则后者在前者右边，调整后方向为1
            {
                Change_dir(dir_bef, 1); // 调整车头方向
                dir_bef = 1;
                OpenMV_Track(50); // 车辆循迹
                delay(100);
                DCMotor.Go(Go_Speed, Go_Temp); // 车辆前进
                delay(500);
            }

            else if (choose == 4 && tracking == 1) // 后者行坐标小于前者，则后者在前者下方，调整后方向为2
            {
                Change_dir(dir_bef, 2); // 调整车头方向
                dir_bef = 2;
                OpenMV_Track(50); // 车辆循迹
                delay(100);
                DCMotor.Go(Go_Speed, Go_Temp); // 车辆前进
            }
            else if (choose == 3 && tracking == 1) // 后者行坐标大于前者，则后者在前者上方，调整后方向为0
            {
                Change_dir(dir_bef, 0); // 调整车头方向
                dir_bef = 0;
                OpenMV_Track(50); // 车辆循迹
                delay(100);
                DCMotor.Go(Go_Speed, Go_Temp); // 车辆前进
            }
        }
        tracking = 1;
    }
    if (link[linked_list].Cd_X == 0)
    {
        Send_Voice_Data(Voice_Finish);
        To_Car(0x1d, 0x00, 0x00, 0x00);
        return;
    }
}

void _TASK::OpenMV_Track(uint8_t Car_Speed)
{
    uint32_t num = 0;
    Car_Speed = 50;
    // 清空串口缓存
    while(ExtSRAMInterface.ExMem_Read(0x6038) != 0x00) ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Usart_buffer, 1); 
    Send_Data_Using_Zigbee(K210_trackdi_open);//开启巡线
    delay(500);
    DCMotor.SpeedCtr(Car_Speed, Car_Speed);
    while(1)
    {
        if(ExtSRAMInterface.ExMem_Read(0x6038) != 0x00)               //检测OpenMV识别结果
        {   
            ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Zigbee_data, 8);        
            if((Zigbee_data[0] == 0x55) && (Zigbee_data[1] == 0x02) && (Zigbee_data[2] == 0x91)) 
            {
                num++;
                Serial.println(num);
                if(Zigbee_data[4] == 1)  // 路口
                {
                    DCMotor.Stop();
                    Send_Data_Using_Zigbee(K210_trackdi_close);//关闭巡线
                    break;
                }
                else  // 调整
                {
                    // if(Zigbee_data[3] == 1)
                    // {
                    //     DCMotor.SpeedCtr(Car_Speed, Car_Speed);
                    // }      
                    if(Zigbee_data[6] <= 5)     // 车身正无需校准
                    {
                        if(Zigbee_data[5] == 43)     // +  向左调
                        {
                            DCMotor.SpeedCtr(Car_Speed, Car_Speed);
                        }
                        else if(Zigbee_data[5] == 45)     // -  向右调
                        {
                            DCMotor.SpeedCtr(Car_Speed, Car_Speed);
                        }
                    }
                    else if(Zigbee_data[6] <= 15)    // 车身微偏
                    {
                        if(Zigbee_data[5] == 43)     // +  向左调
                        {
                            DCMotor.SpeedCtr(20, 50);
                        }
                        else if(Zigbee_data[5] == 45)     // -  向右调
                        {
                            DCMotor.SpeedCtr(50, 20);
                        }
                    }
                    else if(Zigbee_data[6] <= 25)
                    {
                        
                        if(Zigbee_data[5] == 43)     // +  向左调
                        {
                            DCMotor.SpeedCtr(10, 50);
                        }
                        else if(Zigbee_data[5] == 45)     // -  向右调
                        {
                            DCMotor.SpeedCtr(50, 10);
                        } 
                    }
                    else if(Zigbee_data[6] <= 35)
                    {
                        
                        if(Zigbee_data[5] == 43)     // +  向左调
                        {
                            DCMotor.SpeedCtr(5, 50);
                        }
                        else if(Zigbee_data[5] == 45)     // -  向右调
                        {
                            DCMotor.SpeedCtr(50, 5);
                        } 
                    }
                    else if(Zigbee_data[6] <= 50)
                    {
                        
                        if(Zigbee_data[5] == 43)     // +  向左调
                        {
                            DCMotor.SpeedCtr(0, 40);
                        }
                        else if(Zigbee_data[5] == 45)     // -  向右调
                        {
                            DCMotor.SpeedCtr(40, 0);
                        } 
                    }
                    else if(Zigbee_data[6] <= 70)
                    {
                        
                        if(Zigbee_data[5] == 43)     // +  向左调
                        {
                            DCMotor.SpeedCtr(-5, 30);
                        }
                        else if(Zigbee_data[5] == 45)     // -  向右调
                        {
                            DCMotor.SpeedCtr(30, -5);
                        } 
                    }
                    else if(Zigbee_data[6] > 70)
                    {   
                        
                        if(Zigbee_data[5] == 43)     // +  向左调
                        {
                            DCMotor.SpeedCtr(-10, 20);
                        }
                        else if(Zigbee_data[5] == 45)     // -  向右调
                        {
                            DCMotor.SpeedCtr(20, -10);
                        } 
                    }
                }
            }
        }
    }
}

void _TASK::To_Car(uint8_t data, uint8_t data0, uint8_t data1, uint8_t data2)
{

    uint8_t ZigBee_back[8] = {0x00};
    ZigBee_back[0] = 0x55;
    ZigBee_back[1] = 0x02;
    ZigBee_back[2] = data;
    ZigBee_back[3] = data0;
    ZigBee_back[4] = data1;
    ZigBee_back[5] = data2;
    ZigBee_back[7] = 0xbb;
    Command.Judgment(ZigBee_back);
    ExtSRAMInterface.ExMem_Write_Bytes(0x6080, ZigBee_back, 8);
}
void _TASK::Current_Task(uint8_t Task_ID)
{
    switch (Task_ID)
    {
    case 0:

        break;

    case 1:
        Send_Voice_Data(Voice_Jtd);
        // 交通灯识别
        DCMotor.TurnRight(wheel_Speed);
        Traffic_light_recognition();
        DCMotor.TurnLeft(wheel_Speed);
        break;

    case 2:
        // 从车测距
        Send_Voice_Data(Voice_CeJu);
        Ultrasonic_Ranging();

        // 二维码识别
        break;
    case 3:
        Send_Voice_Data(Voice_3);
        break;

    case 4:
        DCMotor.TurnLeft(wheel_Speed);
        // 二维码识别
        Send_Voice_Data(Voice_DZ);
        
        // 道闸
        delay(100);
        Drive.Gate_Show_Zigbee("A88888");
        delay(300);
        Drive.Gate_Show_Zigbee("A88888");
        delay(300);
        DCMotor.CarTrack(Go_Speed);    // 车辆循迹
        DCMotor.Go(Go_Speed, Go_Temp); // 车辆前进
        tracking = 0;
        break;

    case 5:
        Send_Voice_Data(Voice_DaoChe);
        DCMotor.TurnLeft(wheel_Speed);
        // 倒车入库
        DCMotor.CarTrack(Go_Speed); // 车辆循迹
        DCMotor.Back(Go_Speed, 2100);
        tracking = 0;
        break;

    case 6:

        break;

    case 7:

        break;

    case 8:

        break;

    case 9:

        break;

    case 10:
        break;

    case 11:
        break;
    case 12:
        break;

    case 13:
        break;

    case 14:
        break;

    case 15:
        break;

    case 16:
        break;

    case 17:
        break;
    }
}
/************************************************************************************************************
【函 数 名】：	Send_Data_Using_Zigbee		发送zigbee数据给标志物
【参数说明】：	依次为帧头第二位，主指令，三位副指令
【返 回 值】：	无
【简    例】：
************************************************************************************************************/
void _TASK::Send_Data_Using_Zigbee(uint8_t head, uint8_t primary, uint8_t data0, uint8_t data1, uint8_t data2)
{
    Zigbee_data[0] = 0x55;
    Zigbee_data[1] = head;
    Zigbee_data[2] = primary;
    Zigbee_data[3] = data0;
    Zigbee_data[4] = data1;
    Zigbee_data[5] = data2;
    Command.Judgment(Zigbee_data);
    Zigbee_data[7] = 0xbb;
    ExtSRAMInterface.ExMem_Write_Bytes(0x6008, Zigbee_data, 8);
    delay(200);//zigbee接受延时
    ExtSRAMInterface.ExMem_Write_Bytes(0x6008, Zigbee_data, 8);//二次发送容错
}
/************************************************************************************************************
【函 数 名】：	Send_Data_Using_Zigbee		发送zigbee数据
【参数说明】：	完整指令数组
【返 回 值】：	无
【简    例】：
************************************************************************************************************/
void _TASK::Send_Data_Using_Zigbee(uint8_t *data)
{
    ExtSRAMInterface.ExMem_Write_Bytes(0x6008, data, 8);
    delay(200);
    ExtSRAMInterface.ExMem_Write_Bytes(0x6008, data, 8);
}

void _TASK::Send_Data_Using_Infrared(uint8_t head, uint8_t primary, uint8_t data0, uint8_t data1, uint8_t data2, uint8_t data3)
{
    Infrared_data[0] = head;
    Infrared_data[1] = primary;
    Infrared_data[2] = data0;
    Infrared_data[3] = data1;
    Infrared_data[4] = data2;
    Infrared_data[5] = data3;
    Command.Judgment(Infrared_data);
    Infrare.Transmition(Infrared_data, 6);
    delay(500);
    Infrare.Transmition(Infrared_data, 6);
}
void _TASK::Send_Data_Using_Infrared(uint8_t *data)
{
    Infrare.Transmition(data, 6);
    delay(500);
    Infrare.Transmition(data, 6);
}
void _TASK::Read_Data_K210(uint8_t *data, uint8_t len)
{
    ExtSRAMInterface.ExMem_Read_Bytes(0x6038, data, 8);
}

/************************************************************************************************************
【函 数 名】：	Traffic_light_recognition		交通灯识别
【参数说明】：	null:
【返 回 值】：	无
【简    例】：
************************************************************************************************************/
void _TASK::Traffic_light_recognition()
{
    Send_Data_Using_Zigbee(0x0E, 0x01, 0x00, 0x00, 0x00);                        // 发送指令开启交通灯
    ExtSRAMInterface.ExMem_Write_Bytes(0x6008, Drive.Camera_RedGreen_record, 8); // 摄像头开始调用识别红绿灯方法
    Send_Voice_Data(Voice_Open);
    while (1)
    {
        delay_s(1);

        // 清空串口缓存
        while (ExtSRAMInterface.ExMem_Read(0x6038) != 0x00)
            ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Usart_buffer, 1);
        ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Zigbee_data, 8); // 收取摄像头返回的红绿灯识别结果

        if (Zigbee_data[1] == 0x02 && Zigbee_data[7] == 0xbb)
        {

            if (Zigbee_data[4] == 0x01)
            {
                Send_Voice_Data(Voice_red);
                Send_Data_Using_Zigbee(0x0E, 0x02, 0x01, 0x00, 0x00);
                ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Drive.Camera_RedGreen_record_close, 8); // 关闭红绿灯识别
                Send_Voice_Data(Voice_Traffic_Close);
                break;
            }
            if (Zigbee_data[4] == 0x02)
            {
                Send_Voice_Data(Voice_grepp);
                Send_Data_Using_Zigbee(0x0E, 0x02, 0x02, 0x00, 0x00);//向交通灯返回已识别绿色
                ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Drive.Camera_RedGreen_record_close, 8); // 关闭红绿灯识别
                Send_Voice_Data(Voice_Traffic_Close);
                break;
            }
            if (Zigbee_data[4] == 0x03)
            {
                Send_Voice_Data(Voice_yellow);
                Send_Data_Using_Zigbee(0x0E, 0x02, 0x03, 0x00, 0x00);
                ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Drive.Camera_RedGreen_record_close, 8); // 关闭红绿灯识别
                Send_Voice_Data(Voice_Traffic_Close);
                break;
            }
        }
        // if ((Zigbee_data[0] == 0x55) && (Zigbee_data[1] == 0x02))
        // {
        ExtSRAMInterface.ExMem_Write_Bytes(0x6080, Zigbee_data, 8); // 使用自定义数据区上传OpenMV识别结果
                                                                    // OpenMVRx_Handler(Data_OTABuf);                                        // 接收OpenMV，数据处理函数
        // }
    }
}
void shujichuli(char*arr1,char*arr2)
{
    char jiaoyan[10];
    for(int i=0;i<9;i++)
    {
        if(arr1[i]>=48&&arr1[i]<=57)
        {
            jiaoyan[i]=arr1[i];
        }
    }
    int sum=0;//y，校验和
    for (int i = 0; jiaoyan[i] != '\0'; i++) {
        if (isdigit(jiaoyan[i])) {
            int digit = jiaoyan[i] - '0';  // 将字符转换为对应的数字
            sum += digit;
        }
    }
    for(int i=0;i<6;i++)
    {
        arr2[i]+=sum;
        if(arr2[i]>=90)
        {
            arr2[i]=(arr2[i]-65)%26;
        }
    }

}
/************************************************************************************************************
【函 数 名】：	QR_code_recognition		二维码识别
【参数说明】：	null:
【返 回 值】：	无
【简    例】：
************************************************************************************************************/
void _TASK::QR_code_recognition()
{
    Send_Data_Using_Zigbee(Drive.Camera_QRCode_record);    
    ExtSRAMInterface.ExMem_Write_Bytes(0x6008, Drive.Camera_QRCode_record, 8); // 摄像头开始调用识别二维码方法
    Send_Voice_Data(Voice_QR_Open); // 播报开始二维码识别
    while (1)
    {
        delay_s(1);
        
        while (ExtSRAMInterface.ExMem_Read(0x6038 != 0x00))
            ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Usart_buffer, 1);
        ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Zigbee_data, 8);
        if (Zigbee_data[4] == 0x01 && Zigbee_data[7] == 0xBB)
        {
            Send_Voice_Data(Voice_QR_Close);
            /*
                Zigbee_data[5]  // 传进来的字符串长度
                Zigbee_data[6]  // 传进来的字符串
            */

            // 二维码（1）中信息为固定6个字节长度的字符串,仅包含数字（0~9）。
            if (Zigbee_data[5] == 6)
            {
                /* code */
            }
            
            // 二维码（2）中信息为固定10个字节长度的字符串，仅包含数字（0~9）和大写字母（A~Z）。
            if (Zigbee_data[5] == 10)
            {
                /* code */
            }
            //ExtSRAMInterface.ExMem_Write_Bytes(0x6080, Zigbee_data, 8);
        }
        
    }
}

/************************************************************************************************************
【函 数 名】：	Ultrasonic_Ranging		超声波测距
【参数说明】：	null:
【返 回 值】：	无
【简    例】：
************************************************************************************************************/
void _TASK::Ultrasonic_Ranging()
{
    uint16_t tp = (uint16_t)(Ultrasonic.Ranging(CM) * 10.0);
    Infrared_data[4] = tp / 100;
    Infrared_data[5] = tp % 100;
    Send_Data_Using_Zigbee(0x04, 0x04, 0x00, Infrared_data[4], Infrared_data[5]);
}
/************************************************************************************************************
【函 数 名】：	Send_Voice_Data			        GB2132编码格式语音合成播报函数
【参数说明】：	data							文本对应的GB2132编码，数组长度需大于数据位
【返 回 值】：
【简    例】：
************************************************************************************************************/
void _TASK::Send_Voice_Data(uint8_t *data)
{
    SYN7318.VSPTest(data, 0);
    delay(300);
}

void _TASK::delay_s(uint8_t s)
{
    while (s)
    {
        delay(1000);
        s--;
    }
}
uint8_t _TASK::Quick_reading_FS(uint8_t linked_list)
{
    uint8_t choose; // 当前车的朝向

    if (link[linked_list].Cd_X > link[link[linked_list].vernier].Cd_X)
    {
        choose = 1;
    }
    else if (link[linked_list].Cd_X < link[link[linked_list].vernier].Cd_X)
    {
        choose = 2;
    }
    else
    {
        if (link[linked_list].Cd_Y < link[link[linked_list].vernier].Cd_Y)
        {
            choose = 3;
        }
        else
        {
            choose = 4;
        }
    }

    char arr[16];
    return choose;
}

void _TASK::Change_dir(int dir_b, int dir_a)
{
    if ((dir_b % 2) == (dir_a % 2)) // 判断当前方向与调整后方向是否在一条直线上
    {
        if (dir_b != dir_a) // 方向不一致则调头
        {
            DCMotor.TurnRight(wheel_Speed); // 车辆右转
            DCMotor.TurnRight(wheel_Speed); // 车辆右转
        }

        // else//方向一致直行
        // printf("直行\n"); //现在使用程序会卡死
    }
    else // 不在一条直线上则左转或右转
    {
        if ((dir_a - dir_b == 1) || (dir_a - dir_b == -3))
            DCMotor.TurnRight(wheel_Speed); // 车辆右转
        else
            DCMotor.TurnLeft(wheel_Speed); // 车辆左转
    }
}
